#include "bsp_Tamagawa.h"

UART_HandleTypeDef *tg_uart;
Tamagawa_UartCommTypeDef tg_comm = {
    .processingFlag = 0,
    .timeout_cnt = TAMAGAWA_CNT_NULL,
};

/* 切换到发送模式 */
static void RS485_TransmitEnable(void)
{
    HAL_GPIO_WritePin(TAMAGAWA_RE_GPIO_Port, TAMAGAWA_RE_Pin, GPIO_PIN_SET);
    HAL_GPIO_WritePin(TAMAGAWA_DE_GPIO_Port, TAMAGAWA_DE_Pin, GPIO_PIN_SET);
}

/* 切换到接收模式 */
static void RS485_ReceiveEnable(void)
{
    HAL_GPIO_WritePin(TAMAGAWA_DE_GPIO_Port, TAMAGAWA_DE_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(TAMAGAWA_RE_GPIO_Port, TAMAGAWA_RE_Pin, GPIO_PIN_RESET);
}

/* 初始化函数 */
void Tamagawa_Init(UART_HandleTypeDef *huart)
{
    /* 临时方案，用于给编码器供电 */
    HAL_GPIO_WritePin(GPIOC, GPIO_PIN_13, GPIO_PIN_SET);

    tg_uart = huart;
    /* 默认接收使能 */
    RS485_ReceiveEnable();
}

/* CRC8 校验函数 */
uint8_t Tamagawa_CRC8(const uint8_t *p, uint16_t len)
{
    uint8_t crc = 0x00;
    while (len--) {
        crc ^= *(p++);
    }
    return crc;
}

// 微秒级延时函数（不修改SysTick配置）
static void Tamagawa_delay_us(uint32_t us) {
    uint32_t start_tick, end_tick, curr_tick;
    uint32_t ticks_per_us = 180000000UL / 1000000UL;  // 每微秒的tick数 = 180
    
    // 计算需要等待的总tick数
    uint32_t wait_ticks = us * ticks_per_us;
    
    // 获取当前SysTick计数值（VAL寄存器）
    start_tick = SysTick->VAL;
    
    do {
        // 获取当前计数值
        curr_tick = SysTick->VAL;
        
        // 处理计数器溢出情况（递减计数器的特殊处理）
        if (curr_tick < start_tick) {
            end_tick = start_tick - curr_tick;
        } else {
            end_tick = ((SysTick->LOAD + 1) - curr_tick) + start_tick;
        }
    } while (end_tick < wait_ticks);
}

/* 通用发送 + 接收 */
extern uint32_t g_test_ticks;
static Tamagawa_StatusTypeDef _Tamagawa_Transaction(uint8_t cmd, uint8_t *rxBuf, uint16_t rxLen, uint32_t timeout)
{
    /* 仅发送 1 字节命令 */
    RS485_TransmitEnable();
    if (HAL_UART_Transmit(tg_uart, &cmd, 1, 10) != HAL_OK)
        return TAMAGAWA_ERR;
    /* 等待发送完成 */
    while (__HAL_UART_GET_FLAG(tg_uart, UART_FLAG_TC) == RESET);

    RS485_ReceiveEnable();
    // /* 延时几个微秒 确保电平稳定下来 */
    Tamagawa_delay_us(3);

    // HAL_UART_Receive(&huart6, rxBuf, rxLen, 100);

    /* 校验最后一字节 CRC */
    uint8_t crcCalc = Tamagawa_CRC8(rxBuf, rxLen - 1);
    // if (crcCalc != rxBuf[rxLen - 1])
    //     return TAMAGAWA_CRC_ERR;

    return TAMAGAWA_OK;
}

/* 读取编码器的单圈数据 */
Tamagawa_StatusTypeDef Tamagawa_ReadABS(Tamagawa_ABSDataTypeDef *out, uint32_t timeout)
{
    uint8_t test_buf[6];
    Tamagawa_StatusTypeDef st = _Tamagawa_Transaction(0x02, test_buf, 6, timeout);
    if (st != TAMAGAWA_OK) return st;

    out->CF  = test_buf[0];  // 返回命令
    out->SF  = test_buf[1];  // 状态字节
    out->ABS = (uint32_t)test_buf[4] | ((uint32_t)test_buf[3] << 8) | ((uint32_t)test_buf[2] << 16);  // 单圈数据值

    return TAMAGAWA_OK;
}

/* 读取编码器的ID */
Tamagawa_StatusTypeDef Tamagawa_ReadModelID(Tamagawa_ModelVerTypeDef *out, uint32_t timeout)
{
    uint8_t buf[100];
    Tamagawa_StatusTypeDef st = _Tamagawa_Transaction(0x92, buf, 4, timeout);
    if (st != TAMAGAWA_OK) return st;

    out->CF = buf[0];  // 返回命令
    out->SF = buf[1];  // 状态字节
    out->ID = buf[2];  // 编码器ID
    return TAMAGAWA_OK;
}

/* 读取编码器所有数据 */
uint8_t test_buf[20];
Tamagawa_StatusTypeDef Tamagawa_ReadFull(Tamagawa_FullDataTypeDef *out, uint32_t timeout)
{
    uint8_t buf[11];
    Tamagawa_StatusTypeDef st = _Tamagawa_Transaction(0x1A, test_buf, 11, timeout);
    if (st != TAMAGAWA_OK) return st;

    out->CF   = buf[0]; // 返回命令
    out->SF   = buf[1]; // 状态字节
    out->ABS  = (uint32_t)buf[2] | ((uint32_t)buf[3] << 8) | ((uint32_t)buf[4] << 16); // 单圈数据值
    out->ENID = buf[5]; // 编码器ID
    out->ABM  = (uint32_t)buf[6] | ((uint32_t)buf[7] << 8) | ((uint32_t)buf[8] << 16); // 圈数数据
    out->ALMC = buf[9]; // 故障内容字节
    return TAMAGAWA_OK;
}

/* 读取编码器的错误数据 */
Tamagawa_StatusTypeDef Tamagawa_ReadError(Tamagawa_ResetAbsTypeDef *out,uint32_t timeout)
{
    uint8_t buf[6];
    Tamagawa_StatusTypeDef st = _Tamagawa_Transaction(0xC2, buf, 6, timeout);
    if (st != TAMAGAWA_OK) return st;
		out->CF   = buf[0]; 
    out->SF   = buf[1]; 
    out->DF  = (uint32_t)buf[2] | ((uint32_t)buf[3] << 8) | ((uint32_t)buf[4] << 16); 
    
    // 根据协议解析错误状态字节
    uint8_t errorStatus = buf[1];
    if (errorStatus & 0x01) {
        // 单圈计数错误
    }
    if (errorStatus & 0x02) {
        // 超温、计圈错误、电池报警、电池错误
    }

    

    return TAMAGAWA_OK;
}

/**
 * @brief: 编码器开机自检
 */
Tamagawa_StatusTypeDef Tamagawa_StartCheck(void)
{
    Tamagawa_ModelVerTypeDef ver;

    for (int i = 0; i < 10000 ; i++)
    {
        Tamagawa_StatusTypeDef status1 = Tamagawa_ReadModelID(&ver, 1000);
        if (status1 == TAMAGAWA_OK)
        {
            return TAMAGAWA_OK;
        }
    }
    return TAMAGAWA_TIMEOUT;
}

/**
 * @brief: 发送指令
 */

Tamagawa_StatusTypeDef Tamagawa_SendCmd(Tamagawa_CMD_Type cmd)
{
    Tamagawa_StatusTypeDef res;
    Tamagawa_ModelVerTypeDef tmp1;
    Tamagawa_FullDataTypeDef tmp2;

    if (tg_comm.processingFlag)   
    {
        res = TAMAGAWA_PROCESSING;    // 上一帧指令未处理完成
    }
    else    // 当前不存在未处理完成的指令，可以发送新指令
    {
        switch (cmd)
        {
            case GET_ID:
            {
                tg_comm.sendCmdType = cmd;
                tg_comm.processingFlag = 1;
                tg_comm.timeout_cnt = 0;
                res = Tamagawa_ReadModelID(&tmp1, 100);
            }
            break;

            case GET_ALL:
            {
                tg_comm.sendCmdType = cmd;
                tg_comm.processingFlag = 1;
                tg_comm.timeout_cnt = 0;
                res = Tamagawa_ReadFull(&tmp2, 100);
            }
            break;

            default:
            {
                res = TAMAGAWA_CMD_INVAILD;    // 指令码无效
            }
            break;
        }
    }

    return res;
}

uint8_t get_CmdResponseLength(Tamagawa_CMD_Type cmd)
{
    uint8_t res = 0;

    switch(cmd)
    {
        case GET_ID:
        {
            res = 10;
        }
        break;

        case GET_ALL:
        {
            res = 11;
        }
        break;

        default:
        {
            res = 0;
        }
        break;
    }

    return res;
}

void Tamagawa_Timer(void)
{
    if (tg_comm.timeout_cnt != TAMAGAWA_CNT_NULL)
    {
        tg_comm.timeout_cnt ++;

        if (tg_comm.timeout_cnt > 10)   // 接收超时
        {
            tg_comm.processingFlag = 0;
            tg_comm.timeout_cnt = TAMAGAWA_CNT_NULL;
        }
    }
}
